/* Linear interpolation using least squares */
#include "header.h"

/* -------------------------------------------------------------------------- */
void interpolate(int point, struct node* p){
/* -------------------------------------------------------------------------- */
int i, j, k, l, neigh, LSIZE;
double dx, dy, dz, ds2, weight[100], **Lmat, Amat[100][4];
double sr[5], srdx[5], srdy[5], srdz[5], b[4];


LSIZE = 4;
Lmat = (double**)calloc(LSIZE,sizeof(double*));
for(i=0;i<LSIZE;i++) Lmat[i] = (double*)calloc(LSIZE,sizeof(double));
b[0] = b[1] = b[2] = b[3] = 1.0;

for(k=0;k<5;k++)
	sr[k] = srdx[k] = srdy[k] = srdz[k] = 0.0;

for(j=0;j<p[point].total_conn;j++){
	neigh = p[point].Conn[j];
	dx    = p[neigh].x - p[point].x;
	dy    = p[neigh].y - p[point].y;
	dz    = p[neigh].z - p[point].z;
	ds2 = dx*dx + dy*dy + dz*dz;
	weight[j] = WEIGHT(ds2,p[point].ds2);
	Amat[j][0] = 1.0;
	Amat[j][1] = dx;
	Amat[j][2] = dy;
	Amat[j][3] = dz;
	for(k=0;k<5;k++){
		sr[k]   += weight[j]*p[neigh].var[k];
		srdx[k] += weight[j]*p[neigh].var[k]*dx;
		srdy[k] += weight[j]*p[neigh].var[k]*dy;
		srdz[k] += weight[j]*p[neigh].var[k]*dz;
		}
	}

for(j=0;j<LSIZE;j++)
	for(k=0;k<LSIZE;k++){
		Lmat[j][k] = 0.0;
		for(l=0;l<p[point].total_conn;l++)
			Lmat[j][k] += Amat[l][j] * weight[l] * Amat[l][k];
		}
Gaussj(Lmat,LSIZE,b);

for(k=0;k<5;k++)
p[point].var[k] = Lmat[0][0]*sr[k] + Lmat[0][1]*srdx[k] + Lmat[0][2]*srdy[k]
                  + Lmat[0][3]*srdz[k];

if(p[point].var[0] <= 0.0){
	printf("interpolate: Density is negative at node = %d\n", point);
	exit(0);
	}
if(p[point].var[4] <= 0.0){
	printf("interpolate: Pressure is negative at node = %d\n", point);
	exit(0);
	}
for(i=0;i<LSIZE;i++) free(Lmat[i]); free(Lmat);
}
